h = 1.4;                        % Step size
t = 0:h:100;                    % Calculates step over time; upto a maximum value)

omega = zeros(1, length(t));    % Create an empty array for omega 
omega(1) = 0.25;                % Initialise the first index of the array to zero

theta = zeros(1, length(t));    % Create an empty array for theta
theta(1) = 0;                   % Initialise the first index of the array to zero

x = zeros(1, length(t));        % Create an empty array for X
x(1) = 0;                       % Initialise the first index of the array to zero

y = zeros(1, length(t));        % Create an empty array for Y
y(1) = 0;                       % Initialise the first index of the array to zero

z = zeros(1, length(t));        % Create an empty array for Z
z(1) = 0;                       % Initialise the first index of the array to zero

for i = 1:(length(t)-1)         % Loop through array
    
    % Calculate Constants
    oK1 = ode(omega(i), theta(i));                                % Get K1 constant value
    oK2 = ode(omega(i) + 0.5 * h, theta(i) + 0.5 * h * oK1);      % Get K2 constant using K1 value
    oK3 = ode((omega(i) + 0.5 * h), (theta(i) + 0.5 * h * oK2));  % Get K3 constant using K2 value
    oK4 = ode((omega(i) + h), (theta(i) + oK3 * h));              % Get K4 using all previous K values

    % Calculate omega and theta values
    omega(i+1) = omega(i) + h/6 * (oK1 + (2 * oK2) + (2 * oK3) + oK4);
    theta(i+1) = theta(i) + omega(i+1) * h;
    
    % Calculate x, y and z values
    x(i) = 100 * cos(theta(i));
    y(i) = 100 * sin(theta(i));
    z(i) = 10*0.5 * cos(4 * theta(i));
    
end

close all;
figure;plot(theta, t);      % Car Angle over time
figure;plot(omega, t);      % Car Velocity over time
figure;plot(alpha, t);      % Car Acceleration over time
figure;plot3(x, y, z);      % Car Position in 3D space
figure;plot(x, y);          % Car Position in 2D space
figure;plot(x, t);          % Car X Position over time
figure;plot(y, t);          % Car Y Position over time
figure;plot3(x, y, theta);  % Car Orientation; X and Y Positions with Angle